/*
 * garage.c 车库无用
 *
 *  Created on: 2023骞�5鏈�12鏃�
 *      Author: 0
 */

#include "zf_common_headfile.h"

enum garage_type_e garage_type = GARAGE_NONE;

uint8 line_status = 0, last_line_status = 0;

int gipts0[POINTS_MAX_LEN][2]; // 在原图上车库左线的坐标
int gipts1[POINTS_MAX_LEN][2]; // 在原图上车库右线的坐标
int gipts0_num, gipts1_num;

float grpts0[POINTS_MAX_LEN][2];  // 逆透视后的左线坐标
float grpts1[POINTS_MAX_LEN][2];  // 逆透视后的右线坐标
int grpts0_num, grpts1_num;

float grpts0b[POINTS_MAX_LEN][2];  // 滤波后的逆透视左线坐标
float grpts1b[POINTS_MAX_LEN][2];  // 滤波后的逆透视右线坐标
int grpts0b_num, grpts1b_num;

float grpts0s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视左线坐标
float grpts1s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视右线坐标
int grpts0s_num, grpts1s_num;

float grpts0a[POINTS_MAX_LEN];  // 左线局部角变化率,弧度
float grpts1a[POINTS_MAX_LEN];  // 右线局部角变化率,弧度
int grpts0a_num, grpts1a_num;

float grpts0an[POINTS_MAX_LEN];  // 左线角变化率非极大抑制
float grpts1an[POINTS_MAX_LEN];  // 右线角变化率非极大抑制
int grpts0an_num, grpts1an_num;

float grptsc0[POINTS_MAX_LEN][2];  // 左中线
float grptsc1[POINTS_MAX_LEN][2];  // 右中线
int grptsc0_num, grptsc1_num;

int g_Lpt1_c_x, g_Lpt1_c_y, g_temp_line, g_temp_l, g_temp_2;
int Lpt0_garage_id = 0, Lpt1_garage_id = 0, gx1 = 0, gy1 = 0, gx0 = 0, gy0 = 0;
bool Lpt0_garage_found, Lpt1_garage_found;

// 车库检查
void garage_check (void)
{
    // 判断是否为左车库
    if (garage_type == GARAGE_NONE && Lpt0_found_left && Lpt0_rpts0s_id_left < (0.3 / sample_dist)
            && (!Lpt0_found_right) && (!Lpt1_found_left) && (!Lpt1_found_right) && is_straight1)
    {
        // 循右线,进左库
        track_type = TRACK_RIGHT;
        if (check_jump_point(0) > 8)
        {
            garage_type = GARAGE_LEFT_IN;
            car_flag=4;
        }
    }

    // 判断是否为右车库
    if (garage_type == GARAGE_NONE && Lpt1_found_right && Lpt1_rpts1s_id_right < (0.3 / sample_dist)
            && (!Lpt1_found_left) && (!Lpt0_found_left) && (!Lpt0_found_right) && is_straight0)
    {
        // 循左线,进右库
        track_type = TRACK_LEFT;
        if (check_jump_point(1) > 8)
        {
            garage_type = GARAGE_RIGHT_IN;
            car_flag=1;
        }
    }
}

// 检测车库前斑马线
int check_jump_point (char mode)
{
    // 存放查找的行列范围
    int Lpt_x = 0, Lpt_y = 0, lpt_x = 0, lpt_y = 0;
    // 记录跳点个数
    int jump_point = 0;

    switch (mode)
    {
    case 0 :
        // 获取左右边线第一个/第二个向上偏移一段距离的点在原始图像上的坐标
        inv_Perspective(rpts0s[Lpt0_found_left][0], (rpts0s[Lpt0_found_left][1] - 0.22 * pixel_per_meter), &Lpt_x, &Lpt_y);
        inv_Perspective(rpts1s[Lpt0_found_left][0], (rpts1s[Lpt0_found_left][1] - 0.22 * pixel_per_meter), &lpt_x, &lpt_y);

        // 在同一行上从左点遍历至右点,记录跳点
        for (int i = Lpt_x; i < lpt_x; i++)
        {
            if (AT_IMAGE(&img_raw, i, Lpt_y) < thres)
                line_status = 1;
            else if (AT_IMAGE(&img_raw, i, Lpt_y) > thres)
                line_status = 0;

            if (line_status != last_line_status)
                jump_point++;
            last_line_status = line_status;
        }
        break;
    case 1 :
        // 获取左右边线第一个/第二个点向上偏移一段距离的在原始图像上的坐标
        inv_Perspective(rpts1s[Lpt1_found_right][0], (rpts1s[Lpt1_found_right][1] - 0.22 * pixel_per_meter), &Lpt_x, &Lpt_y);
        inv_Perspective(rpts0s[Lpt1_found_right][0], (rpts0s[Lpt1_found_right][1] - 0.22 * pixel_per_meter), &lpt_x, &lpt_y);

        // 在同一行上从右点遍历至左点,记录跳点
        for (int i = Lpt_x; i > lpt_x; i--)
        {
            if (AT_IMAGE(&img_raw, i, Lpt_y) < thres)
                line_status = 1;
            else if (AT_IMAGE(&img_raw, i, Lpt_y) > thres)
                line_status = 0;

            if (line_status != last_line_status)
                jump_point++;
            last_line_status = line_status;
        }
        break;
    case 2 :
            inv_Perspective(rpts0s[Lpt0_found_left][0], (float) clip((int) (rpts0s[Lpt0_found_left][1] + 0.12 * pixel_per_meter), 0, USED_ROW - 5), &Lpt_x, &Lpt_y);
            inv_Perspective(rpts1s[Lpt0_found_left][0], (float) clip((int) (rpts1s[Lpt0_found_left][1] + 0.12 * pixel_per_meter), 0, USED_ROW - 5), &lpt_x, &lpt_y);

            // 在同一行上从左点遍历至右点,记录跳点
            for (int i = Lpt_x; i < lpt_x; i++)
            {
                if (AT_IMAGE(&img_raw, i, Lpt_y) < thres)
                    line_status = 1;
                else if (AT_IMAGE(&img_raw, i, Lpt_y) > thres)
                    line_status = 0;

                if (line_status != last_line_status)
                    jump_point++;
                last_line_status = line_status;
            }
            break;
    }
    return jump_point;
}

enum stop_car_type_e stop_car_type = STOP_NONE;

// 停车状态标志位名称
const char *stop_car_type_name[STOP_NUM] = {
        "STOP_NONE",
        "STOP_CAR "
};

// 停车时长计算常量
float stop_aim_distance = 4500;
// 停车时长
int stop_time = 0;
float stop_speed_step = 0.0;

// 检查是否为斑马线
void check_stop_car()
{
    if (JOIN_OTHER_STATE && check_jump_point(2) > 8)
    {
        stop_car_type = STOP_CAR;
        stop_time = stop_aim_distance / set_speed;
        stop_speed_step = set_speed / stop_time;
    }
}

// 停车
void stop_car()
{
    if (stop_car_type == STOP_CAR)
    {
        if (IfxCpu_acquireMutex(&g_MutexUart))
        {
            ips200_show_int(170, 5, set_speed, 4);
            IfxCpu_releaseMutex(&g_MutexUart);
        }
        if (stop_time != 0)
        {
            --stop_time;
            set_speed -= stop_speed_step;
        }
        else
        {
            protect_flag = 0;
            stop_car_type = STOP_NONE;
        }
    }
}

void run_garage (void)
{
    uint8 value = 0;

    if (garage_type == GARAGE_RIGHT_IN)
    {
        gipts1_num = sizeof(gipts1) / sizeof(gipts1[0]);
        /*   if (ipts1_num != 0)
         {
         gx1 = clip((ipts1[ipts1_num - 2][0] - 2),0,img_raw.width);
         gy1 = ipts1[ipts1_num - 2][1];
         g_temp_2=gx1;
         g_temp_l = gy1;
         }*/
        g_temp_2 = 170;
        g_temp_l = 60;

        for (gx1 = g_temp_2, gy1 = g_temp_l; gy1 - 1 > block_size; gy1--)
        {
            if (AT_IMAGE(&img_raw, gx1, gy1-1) < thres)
                break;
        }

        findline_righthand_adaptive(&img_raw, block_size, thres, gx1, gy1, gipts1, &gipts1_num, 0);

        if (gipts1_num != 0)
        {
            for (int i = 0; i < gipts1_num; i++)
            {
                value = ImagePerspective(gipts1[i][0], gipts1[i][1], &grpts1[i][0], &grpts1[i][1]);
                if (value == 1)
                    break;
                grpts1_num = i;
            }
        }
        else
            grpts1_num = 0;

        blur_points(grpts1, grpts1_num, grpts1b, (int) round(line_blur_kernel));
        grpts1b_num = grpts1_num;

        grpts1s_num = sizeof(grpts1s) / sizeof(grpts1s[0]);
        resample_points2(grpts1b, grpts1b_num, grpts1s, &grpts1s_num, sample_dist * pixel_per_meter);

        local_angle_points(grpts1s, grpts1s_num, grpts1a, (int) round(angle_dist / sample_dist));
        grpts1a_num = grpts1s_num;

        gipts0_num = sizeof(gipts0) / sizeof(gipts0[0]);

        find_circle_corners(2, grpts1a, grpts1a_num, &Lpt1_garage_id);

        inv_Perspective(grpts1s[Lpt1_garage_id][0], grpts1s[Lpt1_garage_id][1], &g_Lpt1_c_x, &g_Lpt1_c_y);

        g_temp_line = ImageAddingLine(gipts0, 37, 30, g_Lpt1_c_x, g_Lpt1_c_y, 0, gipts0_num);        //////鍙傛暟

        for (int i = g_temp_line, j = Lpt1_garage_id;; i++, j--)
        {
            if (j > 0 && i < gipts0_num)
            {
                inv_Perspective(grpts1s[j][0], grpts1s[j][1], &gipts0[i][0], &gipts0[i][1]);
            }
            else
            {
                gipts0_num = i;
                break;
            }
        }

        if (gipts0_num != 0)
        {
            for (int i = 0; i < gipts0_num; i++)
            {
                value = ImagePerspective(gipts0[i][0], gipts0[i][1], &grpts0[i][0], &grpts0[i][1]);
                if (value == 1)
                    break;
                grpts0_num = i;
            }
        }
        else
            grpts0_num = 0;

        // 杈圭嚎婊ゆ尝
        blur_points(grpts0, grpts0_num, grpts0b, (int) round(line_blur_kernel));
        grpts0b_num = grpts0_num;

        // 杈圭嚎绛夎窛閲囨牱
        grpts0s_num = sizeof(grpts0s) / sizeof(grpts0s[0]);
        resample_points2(grpts0b, grpts0b_num, grpts0s, &grpts0s_num, sample_dist * pixel_per_meter);

        // 杈圭嚎灞�閮ㄨ搴﹀彉鍖栫巼
        //local_angle_points(crpts0s, crpts0s_num, crpts0a, (int) round(angle_dist / sample_dist));
        //  crpts0a_num = crpts0s_num;

        // 瑙掑害鍙樺寲鐜囬潪鏋佸ぇ鎶戝埗
        //nms_angle(crpts0a, crpts0a_num, crpts0an, (int) round(angle_dist / sample_dist) * 2 + 1);
        //crpts0an_num = crpts0a_num;

        // 宸﹀彸涓嚎璺熻釜
        track_leftline(grpts0s, grpts0s_num, grptsc0, (int) round(angle_dist / sample_dist),
                pixel_per_meter * ROAD_WIDTH / 2);
        grptsc0_num = grpts0s_num;

        track_type = TRACK_GARAGE_LEFT;

        if (Lpt1_garage_id == false || ipts0_num > 10)
        {
            garage_type = GARAGE_RIGHT_END;
            track_type = TRACK_LEFT;
        }

    }
    else if (garage_type == GARAGE_RIGHT_END)
    {
        if (rptsc1_num < 10 && rptsc0_num < 10)
        {
            garage_type = GARAGE_NONE;
            protect_flag = 0;
        }
    }

}
